﻿#include "robotarmmonitor.h"

RobotArmMonitor::RobotArmMonitor(QObject *parent) : QObject(parent)
{
    qDebug() << "RobotArmMonitor thread " << this->thread();
    monitor_socket_ = new QTcpSocket(this);
}


bool RobotArmMonitor::connectToMonitorHost(std::string host, int port){
    //qDebug() << "connectTo Robot arm Host! thread is " << this->thread();

    if(monitor_socket_ == nullptr){
        return false;
    }

    qDebug() << "connectToMonitorHost thread " << this->thread();
    if(monitor_socket_->isOpen())
    {
        return true;
    }

    //socket.connectToHost(host, port); //连接机械臂
    monitor_socket_->connectToHost(QString::fromStdString(host), port);
    if(!monitor_socket_->waitForConnected(3000))
    {
        return false;
    }

    QByteArray data = monitor_socket_->read(1024);
    return true;
}

void RobotArmMonitor::update_joint_pose(QByteArray& data){

    QByteArray joint1 = data.mid(13,8);
    QByteArray joint2 = data.mid(21,8);
    QByteArray joint3 = data.mid(29,8);
    QByteArray joint4 = data.mid(37,8);
    QByteArray joint5 = data.mid(45,8);
    QByteArray joint6 = data.mid(53,8);

    std::reverse(joint1.begin(),joint1.end());
    std::reverse(joint2.begin(),joint2.end());
    std::reverse(joint3.begin(),joint3.end());
    std::reverse(joint4.begin(),joint4.end());
    std::reverse(joint5.begin(),joint5.end());
    std::reverse(joint6.begin(),joint6.end());

    double d_joint1,d_joint2,d_joint3,d_joint4,d_joint5,d_joint6;

    memcpy(&d_joint1,joint1.data(),sizeof(d_joint1));
    memcpy(&d_joint2,joint2.data(),sizeof(d_joint2));
    memcpy(&d_joint3,joint3.data(),sizeof(d_joint3));
    memcpy(&d_joint4,joint4.data(),sizeof(d_joint4));
    memcpy(&d_joint5,joint5.data(),sizeof(d_joint5));
    memcpy(&d_joint6,joint6.data(),sizeof(d_joint6));

    robot_joint_pose[0] = d_joint1;
    robot_joint_pose[1] = d_joint2;
    robot_joint_pose[2] = d_joint3;
    robot_joint_pose[3] = d_joint4;
    robot_joint_pose[4] = d_joint5;
    robot_joint_pose[5] = d_joint6;

    //qDebug() << "robot_joint_pose " << robot_joint_pose[0]
    //         << " " << robot_joint_pose[1]
    //         << " " << robot_joint_pose[2]
    //         << " " << robot_joint_pose[3]
    //         << " " << robot_joint_pose[4]
    //         << " " << robot_joint_pose[5];
}

void RobotArmMonitor::update_xyz_pose(QByteArray& data){

    QByteArray x = data.mid(77,8);
    QByteArray y = data.mid(85,8);
    QByteArray z = data.mid(93,8);
    QByteArray rx = data.mid(101,8);
    QByteArray ry = data.mid(109,8);
    QByteArray rz = data.mid(117,8);

    std::reverse(x.begin(),x.end());
    std::reverse(y.begin(),y.end());
    std::reverse(z.begin(),z.end());
    std::reverse(rx.begin(),rx.end());
    std::reverse(ry.begin(),ry.end());
    std::reverse(rz.begin(),rz.end());

    double d_x,d_y,d_z,d_rx,d_ry,d_rz;

    memcpy(&d_x,x.data(),sizeof(d_x));
    memcpy(&d_y,y.data(),sizeof(d_y));
    memcpy(&d_z,z.data(),sizeof(d_z));
    memcpy(&d_rx,rx.data(),sizeof(d_rx));
    memcpy(&d_ry,ry.data(),sizeof(d_ry));
    memcpy(&d_rz,rz.data(),sizeof(d_rz));

    robot_xyz_pose[0] = d_x;
    robot_xyz_pose[1] = d_y;
    robot_xyz_pose[2] = d_z;
    robot_xyz_pose[3] = d_rx * 180 / 3.1415926;;
    robot_xyz_pose[4] = d_ry * 180 / 3.1415926;;
    robot_xyz_pose[5] = d_rz * 180 / 3.1415926;;

//    qDebug() << "robot_xyz_pose " << robot_xyz_pose[0]
//             << " " << robot_xyz_pose[1]
//             << " " << robot_xyz_pose[2]
//             << " " << robot_xyz_pose[3]
//             << " " << robot_xyz_pose[4]
//             << " " << robot_xyz_pose[5];
}

void RobotArmMonitor::update_robot_state(QByteArray& data){

    QByteArray state = data.mid(237,4);
    std::reverse(state.begin(),state.end());

    int i_state = -1;
    memcpy(&i_state,state.data(),sizeof(i_state));

    robot_state = i_state;
    if(i_state < 0 || i_state > 4){
         qDebug() << "i_state " << i_state;
    }

}

void RobotArmMonitor::update_robot_mode(QByteArray& data){
    QByteArray state = data.mid(281,4);
    std::reverse(state.begin(),state.end());

    int i_state = -1;
    memcpy(&i_state,state.data(),sizeof(i_state));

    robot_mode = i_state;
    if(i_state < 0 || i_state > 2){
            qDebug() << "mode " << i_state;\
    }

}


bool RobotArmMonitor::check_size(QByteArray& data){
    QByteArray state = data.mid(0,4);
    std::reverse(state.begin(),state.end());

    int size = -1;
    memcpy(&size,state.data(),sizeof(size));

    //qDebug() << "size " << size;

    if(size != 1024){
        //qDebug() << "wrong " << state;
        return false;
    }

    return true;
}


void RobotArmMonitor::run_monitor(){

    qDebug() << "run_monitor thread " << this->thread();

    connectToMonitorHost(host_ip.toStdString(),8056);

    run_flag = true;

    //monitor_socket_->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption,1024);//设置缓冲区
    //monitor_socket_->setReadBufferSize(1024);
    while (run_flag) {

       monitor_socket_->waitForReadyRead(10);

       //QByteArray response;
       if(monitor_socket_->bytesAvailable()>0)
       {
           //qDebug() << "monitor_socket_->bytesAvailable() " << monitor_socket_->bytesAvailable();
           QByteArray datagram;
           datagram.resize(monitor_socket_->bytesAvailable());
           monitor_socket_->read(datagram.data(),datagram.size());

           //qDebug() << "response size " << response.size();
           //qDebug() << "readBufferSize " << monitor_socket_->readBufferSize();

           if(datagram.size() == 2048){

               //check size
               if(check_size(datagram)){

               //update joint
               update_joint_pose(datagram);
               //update_xyz
               update_xyz_pose(datagram);
               //update_state
               update_robot_state(datagram);
               //
               update_robot_mode(datagram);

               }
           }
       }
       else{
           //qDebug() << "response error size " << datagram.size();
       }

       QThread::msleep(10);

       //response.clear();
    }
}


//
void RobotArmMonitor::slot_finish(){

}

